// Copyright(c) 2011 Matthew McMullan, Jorel Lalicki and Mike Heise, Jorel Lalicki and Mike Heise. See copying.txt for details.

#ifndef __agml_sin_h__
#define __agml_sin_h__

// make sure that this file is never included directly
#ifndef __agml_agml_h__
#error "Include AGML from agml.h only. This file is not a valid entry point for the library."
#endif

#include <math.h>   /*  fabsf(3)  */

namespace agml {

inline f32 _sin(f32 theta);
inline f32 _csc(f32 theta);

/*!  parabolic fast sine approximation with good error bounds */
inline f32 sin(f32 theta) {
	return _sin( _mod(theta, (f32)_pi) );
//        return _sin(fmodf(theta, (f32)_pi));
}

/*!  faster sine approximation with domain restricted to [-pi, pi]  */
f32 _sin(f32 theta) {
    static const f32 upper = 4.0f/(f32)_pi;
    static const f32 lower = -4.0f/((f32)_pi*(f32)_pi);
    static const f32 scale = 0.225f;  //empirical
    static const f32 iscale= 1.0f - scale;

	//first parabolic approx
	f32 f = (theta  * upper) + (theta * lower * fabsf(theta));

	//second approximation: average with quartic
	return (scale * fabsf(f) + iscale) * f;
}

inline void _sin(_v128 &theta) {
	// for programmar sanity only, should be removed at compile time
    static const f32 _upper = 4.0f/(f32)_pi;
    static const f32 _lower = -4.0f/((f32)_pi*(f32)_pi);
    static const f32 _scale = 0.225f;  //empirical
	// the real ones
	static const _v128 upper = {_upper,_upper,_upper,_upper};
	static const _v128 lower = {_lower,_lower,_lower,_lower};
	static const _v128 scale = {_scale,_scale,_scale,_scale};
	// create a bitmask to use for calculating the absolute value
	union {
		f32 stupid;
		u32 bitmask;
	};
	bitmask = 0x7FFFFFFF;	// 0 followed by 31 1s
	_v128 absolute = {stupid,stupid,stupid,stupid};
	// calculate the first parabolic approximation
	_v128 tmp = _mm_and_ps(absolute,theta);
	theta = tmp;
	tmp = _mm_mul_ps(tmp,theta);
	tmp = _mm_mul_ps(tmp,lower);
	theta = _mm_mul_ps(theta,upper);
	theta = _mm_add_ps(theta,tmp);
	// calculate the second parabolic approximation
	tmp = _mm_and_ps(theta,absolute);
	tmp = _mm_mul_ps(tmp,theta);
	tmp = _mm_sub_ps(tmp,theta);
	tmp = _mm_mul_ps(tmp,scale);
	theta = _mm_add_ps(theta,tmp);
}

/*! fast cosecant approximation based on sine approximation  */
inline f32 csc(f32 theta) {
    return 1.0f/sin(theta);
}


/*!  actual version with domain restricted to (-pi, pi)  */
inline f32 _csc(f32 theta) {
    return 1.0f/_sin(theta);
}

/*!	simd cosecant with same domain restriction	*/
inline void _csc(_v128 &thetas) {
	_sin(thetas);
	_mm_rcp_ps(thetas);
}


/*! polynomial fast arcsine approximation, error bounds of ~1.5% until within 1%
 * of domain bounds */
inline f32 asin(f32 x) {
    static const f32 scale_factor = .391f;   //empirical
    f32 x5 = x * x;     //x^2
    x5 *= x5;           //x^4
    x5 *= x;            //x^5
    return x + scale_factor*x5;
}

/*!	simd arcsine	*/
inline void asin(_v128 &x) {
	static const f32 sf = .391f;
	static const _v128 scale_factor = {sf, sf, sf, sf};
	_v128 x5 = x;
	x5 = _mm_mul_ps(x5, x);		//x^2
	x5 = _mm_mul_ps(x5, x5);	//x^4
	x5 = _mm_mul_ps(x5, x);		//x^5
	x5 = _mm_mul_ps(x5, scale_factor);
	x = _mm_add_ps(x5, x);
}

}
#endif
